library IEEE;
use IEEE.std_logic_1164.all;
use IEEE.numeric_std.all;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;

entity CORDIC is 
	port( 
			Xin		 	: in std_logic_vector(14 downto 0); 
		   Yin			: in std_logic_vector(14 downto 0);
		   Zin			: in std_logic_vector(8 downto 0); 
		   Xout		 	: out std_logic_vector(14 downto 0);
		   Yout		 	: out std_logic_vector(14 downto 0)
	);
end CORDIC;

-- Las entradas entran en complemento a 2

architecture cordic of CORDIC is
begin
	process(Xin,Yin,Zin)
	    TYPE std_logic_matrix IS ARRAY (NATURAL RANGE <>)OF std_logic_vector(14 downto 0);
	    TYPE mem_matrix IS ARRAY (NATURAL RANGE <>)OF std_logic_vector(8 downto 0);
		 
	    constant at: mem_matrix (0 to 6):=(
			std_logic_vector(to_signed(64,9)),
			std_logic_vector(to_signed(38,9)),
			std_logic_vector(to_signed(20,9)),
			std_logic_vector(to_signed(10,9)),
			std_logic_vector(to_signed(5,9)),
			std_logic_vector(to_signed(2,9)),
			std_logic_vector(to_signed(1,9)));
	    
		 constant ganancia: std_logic_vector(12 downto 0):=std_logic_vector(to_unsigned(6073,13));
		 
	    variable Xaux: std_logic_matrix (0 to 7);
	    variable Yaux: std_logic_matrix (0 to 7);
	    variable Zaux: mem_matrix (0 to 7);
		 
	    variable Prod: std_logic_vector (27 downto 0);
		 
	begin
	     if Zin/=(8 downto 0 =>'0') then
	        Xaux(0):=Xin;
	        Yaux(0):=Yin;
	        Zaux(0):=Zin;
	        if Zaux(0)(0)='0'  then --d=1
	              Xaux(1):=Xaux(0)-Yaux(0);
	              Yaux(1):=Yaux(0)+Xaux(0);
	              Zaux(1):=Zaux(0)-at(0);
	        else --d=-1
	              Xaux(1):=Xaux(0)+Yaux(0);
	              Yaux(1):=Yaux(0)-Xaux(0);
	              Zaux(1):=Zaux(0)+at(0);
	        end if;
	        for i in 1 to 6 loop
	           if Zaux(i)(0)='0' then --d=1
	              Xaux(i+1):=Xaux(i)-((14 downto 15-i => Yaux(i)(14))&(Yaux(i)(14 downto i)));
	              Yaux(i+1):=Yaux(i)+((14 downto 15-i => Xaux(i)(14))&(Xaux(i)(14 downto i)));
	              Zaux(i+1):=Zaux(i)-at(i);
	           else --d=-1
	              Xaux(i+1):=Xaux(i)+((14 downto 15-i => Yaux(i)(14))&(Yaux(i)(14 downto i)));
	              Yaux(i+1):=Yaux(i)-((14 downto 15-i => Xaux(i)(14))&(Xaux(i)(14 downto i)));
	              Zaux(i+1):=Zaux(i)+at(i);
	           end if;
	        end loop;
	        if Xaux(7)(14)='0' then
	           Prod:='0' & (ganancia*(Xaux(7)(13 downto 0)));
	        else
	           Xaux(7):=not(Xaux(7))+1;
	           Prod:='1' & (not(ganancia*(Xaux(7)(13 downto 0)))+1);
	        end if;
	        Xout<=Prod(27 downto 13);
	        if Yaux(7)(14)='0' then
	           Prod:='0' & (ganancia*(Yaux(7)(13 downto 0)));
	        else
	           Yaux(7):=not(Yaux(7))+1;
	           Prod:='1' & (not(ganancia*(Yaux(7)(13 downto 0)))+1);
	        end if;
	        Yout<=Prod(27 downto 13);
	    else
	        Xout <= Xin;
	        Yout <= Yin;
	    end if;
	end process;
end cordic;